/*
 * @(#)AStar.java     14/08/02
 * 
 * Author             Rick Wilson
 * Copyright (c) 2014 Rick Wilson
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of 'Taareekah' nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */



package util;

//~--- JDK imports ------------------------------------------------------------

import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;
import java.util.Set;
import java.util.Vector;
import java.util.logging.Level;
import java.util.logging.Logger;

import javax.management.JMException;

/**
 *
 * @author rick
 */
public class AStar {

    /** Field description */
    private Vector<AStarNode>	path;

    /** Field description */
    private AStarNode	start, end;

    /** Field description */
    private int	world_size;

    /** Field description */
    private AStarDistanceCalculator	dist_calc;

    /** Field description */
    private AStarHeuristic	heur;

    /**
     *
     * @param dist_calc
     * @param heur
     */
    public AStar(AStarDistanceCalculator dist_calc, AStarHeuristic heur) {
	this.dist_calc	= dist_calc;
	this.heur	= heur;
    }

    /**
     *
     * @return
     */
    public int getPathLength() {
	return path.size();
    }

    /**
     *
     * @param element
     * @return
     * @throws Exception
     */
    public Point getPointOfPath(int element) throws Exception {
	if ((element < 0) || (element >= path.size())) {
	    throw new JMException("Invalid element");
	}

	return path.elementAt(element);
    }

    /**
     *
     * @param element
     * @return
     * @throws Exception
     */
    public int getXOfPathNode(int element) throws Exception {
	if ((element < 0) || (element >= path.size())) {
	    throw new JMException("Invalid element");
	}

	return path.elementAt(element).getX();
    }

    /**
     *
     * @param element
     * @return
     * @throws Exception
     */
    public int getYOfPathNode(int element) throws Exception {
	if ((element < 0) || (element >= path.size())) {
	    throw new JMException("Invalid element");
	}

	return path.elementAt(element).getY();
    }

    /**
     * Method description
     *
     *
     * @param goal
     *
     * @return
     */
    private int reconstructPath(AStarNode goal) {
	Vector<AStarNode>	reverse	= new Vector<AStarNode>();
	AStarNode		current	= goal;

	while (current != null) {
	    reverse.add(current);
	    current	= current.getPrevious();
	}

	reverse.add(start);

	for (int i = reverse.size() - 1; i >= 0; i--) {
	    path.add(reverse.elementAt(i));
	}

	return reverse.size();
    }

    /**
     * Method description
     *
     */
    private void reset() {
	start	= null;
	end	= null;
	path	= null;
    }

    /**
     *
     * @param start_x
     * @param start_y
     * @param goal_x
     * @param goal_y
     * @param world_size
     * @return
     */
    public int findPath(int start_x, int start_y, int goal_x, int goal_y, int world_size) {
	reset();
	this.path	= new Vector<AStarNode>();
	this.world_size	= world_size;
	this.start	= new AStarNode(start_x, start_y);
	this.end	= new AStarNode(goal_x, goal_y);
	Logger.getLogger(AStar.class.getName()).log(Level.INFO, "Finding path from {0}, {1} to {2}, {3}",
			 new Object[] { start_x,
					start_y, goal_x, goal_y });

	PriorityQueue<AStarNode>	open_set	= new PriorityQueue<AStarNode>((world_size * world_size) / 4,
							      new NodeDistanceComparator(end, heur));
	Set<AStarNode>	closed_set	= new HashSet<AStarNode>();

	open_set.add(start);

	int	num_iterations	= 0;

	while ((open_set.size() > 0) && (num_iterations < 1000)) {
	    num_iterations++;

	    AStarNode	current	= open_set.poll();

	    if (current == null) {}

	    if (current.equals(end)) {
		Logger.getLogger(AStar.class.getName()).log(Level.INFO, "{0}", num_iterations);

		return reconstructPath(current);
	    }

	    closed_set.add(current);

	    // Logger.getLogger(AStar.class.getName()).log(Level.INFO, "Here {0}, {1} to {2}, {3}",  new Object[]{start_x, start_y, goal_x, goal_y});
	    for (Point p : current.getNeighborsWithinRange(0, world_size)) {
		AStarNode	v	= new AStarNode(p.getX(), p.getY());

		if (closed_set.contains(v)) {
		    continue;
		}

		if (dist_calc.distanceBetween(current, v) >= 3.0) {
		    continue;
		}

		double	tentative_g_score	= current.getDistanceFromStart()
						  + dist_calc.distanceBetween(current, v);

		if (!open_set.contains(v) || (tentative_g_score < current.getDistanceFromStart())) {
		    v.setDistanceFromStart(tentative_g_score);
		    v.setEstimatedDistanceToGoal(tentative_g_score + heur.estimateDistance(v, end));
		    v.setPrevious(current);

		    if (!open_set.contains(v)) {
			open_set.add(v);
		    }
		}
	    }
	}

	Logger.getLogger(AStar.class.getName()).log(Level.INFO, "Couldnt find path from {0}, {1} to {2}, {3}",
			 new Object[] { start_x,
					start_y, goal_x, goal_y });

	return -1;
    }

    /**
     *
     */
    public class NodeDistanceComparator implements Comparator<AStarNode> {

	/** Field description */
	private AStarNode	goal;

	/** Field description */
	private AStarHeuristic	heur;

	NodeDistanceComparator(AStarNode goal, AStarHeuristic heur) {
	    this.goal	= goal;
	    this.heur	= heur;
	}

	/**
	 * Method description
	 *
	 *
	 * @param l
	 * @param r
	 *
	 * @return
	 */
	public int compare(AStarNode l, AStarNode r) {
	    if (heur.estimateDistance(goal, l) < heur.estimateDistance(goal, r)) {
		return -1;
	    } else if (heur.estimateDistance(goal, l) == heur.estimateDistance(goal, r)) {
		return 0;
	    }

	    return 1;
	}
    }
}


//~ Formatted by Jindent --- http://www.jindent.com
